#include "motor_drivers.hpp"

float MotorDrivers::uint_to_float(int x_int, float x_min, float x_max, int bits){
    /// converts unsigned int to float, given range and number of bits ///
    float span = x_max - x_min;
    float offset = x_min;
    return ((float)x_int)*span/((float)((1<<bits)-1)) + offset;
}

int MotorDrivers::float_to_uint(float x, float x_min, float x_max, int bits){
    /// Converts a float to an unsigned int, given range and number of bits ///
    float span = x_max - x_min;
    float offset = x_min;
    return (int) ((x-offset)*((float)((1<<bits)-1))/span);
}

void MotorDrivers::MotorDM::motor_Enable(){/// 使能帧 ///
    uint8_t data[8];
    for(int i = 0; i < 7; i++){
        data[i] = 0xff;
    }
    data[7] = 0xfc;
    socket_->socket_Send(data);
}

void MotorDrivers::MotorDM::motor_Disable(){/// 失能帧 ///
    uint8_t data[8];
    for(int i = 0; i < 7; i++){
        data[i] = 0xff;
    }
    data[7] = 0xfd;
    socket_->socket_Send(data);
}

void MotorDrivers::MotorDM::motor_CleanPos(){/// 位置标零 ///
    uint8_t data[8];
    for(int i = 0; i < 7; i++){
        data[i] = 0xff;
    }
    data[7] = 0xfe;
    socket_->socket_Send(data);
}

void MotorDrivers::MotorDM::motor_Update(){/// 更新目标位置 ///
    uint8_t data[8];
    uint8_t posBytes[sizeof(float)];
    std::memcpy(posBytes, &targetPos_, sizeof(float));
    for(int i = 0; i < 4; i++){
        data[i] = posBytes[i];
    }

    uint8_t velBytes[sizeof(float)];
    std::memcpy(velBytes, &targetVel_, sizeof(float));
    for (int i = 0; i < 4; ++i) {
        data[i+4] = velBytes[i];
    }

    socket_->socket_Send(data);
    motor_getHWInfo();
}

bool MotorDrivers::MotorDM::motor_getHWInfo(){/// 获取硬件信息 ///
    
    can_frame frame = socket_->frame.can_frame_;
    uint8_t p_int = static_cast<uint8_t>(frame.data[1] << 8) | frame.data[0];
    uint8_t v_int = static_cast<uint8_t>(frame.data[3] << 4) | (frame.data[4] >> 4);
    uint8_t t_int = static_cast<uint8_t>(frame.data[4] &0xF << 8) | frame.data[5];

    crtPos_ = uint_to_float(p_int, P_MAX, P_MIN, 16);
    crtVel_ = uint_to_float(v_int, V_MAX, V_MIN, 12);
    crtTorque_ = uint_to_float(t_int, T_MAX, T_MIN, 12);
    return true;
}